clc;
%       加载数据集
% 时间/s    距离/km   方位/rad
%   5
%   10
%
load('track_data.mat');
% 绘制原始数据图
theta = track_data(:,3);
rho = track_data(:,2);
mea_z_rad = zeros(2,41);
mea_z_rad(1,2:end) = rho;  % r
mea_z_rad(2,2:end) = theta;  %theta
% 转化为笛卡尔坐标系
mea_z = zeros(2,41);
mea_z(1,2:end) = rho .* cos(theta);  %计算x值
mea_z(2,2:end) = rho .* sin(theta);  %计算y值

figure
plot(mea_z(1,2:end),mea_z(2,2:end),'r');
hold on;
%legend('观测数据');
title('ekf');

track = ekf;
track.T = 5;
track.A = [1,0,track.T,0;
           0,1,0,track.T;
           0,0,1,0;
           0,0,0,1];
track.G = [0,0;
           0,0;
           track.T,0;
           0,track.T];
track.Q = [0.001,0;
           0,0.001];
track.R = [0.25,0;
           0,0.0005];
% 指定初始X与P值
X = [20;
     20;
     -0.2;
     0];
 P = [1, 0, 0, 0;
      0, 1, 0, 0;
      0, 0, 0.01, 0;
      0, 0, 0, 0.01];


% 进行ekf
X_apr = zeros(4,41);
X_estimation = zeros(4,41);
Z_err_apr = zeros(2,41);
%%
for i = 1:40
% 预测更新
    [X,P_apr] = track.prediction(X,P);
    X_apr(:,i+1) = X;
       
% 校正
    % [X, P] = track.update(X_apr(:,i+1),P_apr,mea_z_rad(:,i+1));
    Z_apr = track.cal_hz(X_apr(:,i+1));
    z_err = mea_z_rad(:,i+1) - Z_apr;
    Z_err_apr(:,i+1) = z_err;
    
    j_h = track.cal_jacobi(X_apr(:,i+1));   %线性化
    c = j_h * P_apr * j_h' + track.R;
    K = P_apr * j_h' / c ;
    X = X_apr(:,i+1) + K * z_err;
    P = P_apr - K * j_h * P_apr;
    X_estimation(:,i + 1) = X;
end

plot(X_apr(1,2:end),X_apr(2,2:end),'g');
plot(X_estimation(1,2:end),X_estimation(2,2:end),'b');
legend('x measure','x apr','x est');



